/*
 * comath_alg.c
 *
 *  Created on: Apr 29, 2017
 *      Author: yuanjianen
 */

#include "comath_alg.h"

/* lsb: 1/32768 , size: 2049 */
const sint16 comath_svm_table[] = {
	28377,  28428,  28477,  28527,  28576,  28625,  28674,  28723,  28771,  28819,
	28867,  28914,  28961,  29008,  29055,  29101,  29147,  29193,  29238,  29284,
	29329,  29373,  29418,  29462,  29506,  29549,  29593,  29636,  29678,  29721,
	29763,  29805,  29847,  29888,  29929,  29970,  30010,  30051,  30091,  30130,
	30170,  30209,  30247,  30286,  30324,  30362,  30400,  30437,  30474,  30511,
	30548,  30584,  30620,  30655,  30691,  30726,  30761,  30795,  30829,  30863,
	30897,  30930,  30963,  30996,  31029,  31061,  31093,  31124,  31155,  31186,
	31217,  31248,  31278,  31307,  31337,  31366,  31395,  31424,  31452,  31480,
	31508,  31535,  31563,  31589,  31616,  31642,  31668,  31694,  31719,  31744,
	31769,  31794,  31818,  31842,  31865,  31889,  31912,  31934,  31957,  31979,
	32000,  32022,  32043,  32064,  32084,  32105,  32125,  32144,  32164,  32183,
	32202,  32220,  32238,  32256,  32274,  32291,  32308,  32324,  32341,  32357,
	32372,  32388,  32403,  32418,  32432,  32446,  32460,  32474,  32487,  32500,
	32513,  32525,  32537,  32549,  32560,  32572,  32582,  32593,  32603,  32613,
	32623,  32632,  32641,  32650,  32658,  32666,  32674,  32681,  32688,  32695,
	32702,  32708,  32714,  32719,  32725,  32730,  32734,  32739,  32743,  32747,
	32750,  32753,  32756,  32758,  32761,  32763,  32764,  32765,  32766,  32767,
	32767,  32767,  32767,  32767,  32766,  32765,  32763,  32761,  32759,  32757,
	32754,  32751,  32748,  32744,  32740,  32736,  32731,  32726,  32721,  32716,
	32710,  32704,  32697,  32691,  32684,  32676,  32669,  32661,  32652,  32644,
	32635,  32626,  32616,  32606,  32596,  32586,  32575,  32564,  32553,  32541,
	32529,  32517,  32504,  32492,  32478,  32465,  32451,  32437,  32423,  32408,
	32393,  32378,  32362,  32346,  32330,  32313,  32296,  32279,  32262,  32244,
	32226,  32208,  32189,  32170,  32151,  32131,  32111,  32091,  32071,  32050,
	32029,  32008,  31986,  31964,  31942,  31919,  31896,  31873,  31850,  31826,
	31802,  31777,  31753,  31728,  31702,  31677,  31651,  31625,  31598,  31572,
	31544,  31517,  31489,  31462,  31433,  31405,  31376,  31347,  31317,  31288,
	31258,  31227,  31197,  31166,  31135,  31103,  31071,  31039,  31007,  30974,
	30941,  30908,  30875,  30841,  30807,  30772,  30738,  30703,  30667,  30632,
	30596,  30560,  30523,  30487,  30450,  30412,  30375,  30337,  30299,  30260,
	30222,  30183,  30143,  30104,  30064,  30024,  29983,  29943,  29902,  29861,
	29819,  29777,  29735,  29693,  29650,  29607,  29564,  29520,  29477,  29433,
	29388,  29344,  29299,  29254,  29208,  29162,  29116,  29070,  29024,  28977,
	28930,  28882,  28835,  28787,  28739,  28690,  28642,  28593,  28543,  28494,
	28444,  28394,  28277,  28126,  27974,  27823,  27671,  27519,  27366,  27214,
	27061,  26907,  26754,  26600,  26446,  26292,  26138,  25983,  25828,  25673,
	25518,  25362,  25206,  25050,  24894,  24737,  24580,  24423,  24266,  24108,
	23950,  23793,  23634,  23476,  23317,  23158,  22999,  22840,  22681,  22521,
	22361,  22201,  22040,  21880,  21719,  21558,  21397,  21235,  21074,  20912,
	20750,  20588,  20426,  20263,  20100,  19937,  19774,  19611,  19447,  19284,
	19120,  18956,  18792,  18627,  18463,  18298,  18133,  17968,  17803,  17637,
	17472,  17306,  17140,  16974,  16808,  16641,  16475,  16308,  16141,  15974,
	15807,  15640,  15472,  15305,  15137,  14969,  14801,  14633,  14465,  14296,
	14128,  13959,  13790,  13621,  13452,  13283,  13113,  12944,  12774,  12605,
	12435,  12265,  12095,  11925,  11754,  11584,  11413,  11243,  11072,  10901,
	10730,  10559,  10388,  10217,  10046,   9874,   9703,   9531,   9359,   9187,
	9016,   8844,   8672,   8500,   8327,   8155,   7983,   7810,   7638,   7465,
	7293,   7120,   6947,   6774,   6601,   6428,   6255,   6082,   5909,   5736,
	5563,   5389,   5216,   5042,   4869,   4695,   4522,   4348,   4175,   4001,
	3827,   3654,   3480,   3306,   3132,   2958,   2784,   2610,   2436,   2263,
	2089,   1915,   1740,   1566,   1392,   1218,   1044,    870,    696,    522,
	348,    174,      0,   -174,   -348,   -522,   -696,   -870,  -1044,  -1218,
	-1392,  -1566,  -1740,  -1915,  -2089,  -2263,  -2436,  -2610,  -2784,  -2958,
	-3132,  -3306,  -3480,  -3654,  -3827,  -4001,  -4175,  -4348,  -4522,  -4695,
	-4869,  -5042,  -5216,  -5389,  -5563,  -5736,  -5909,  -6082,  -6255,  -6428,
	-6601,  -6774,  -6947,  -7120,  -7293,  -7465,  -7638,  -7810,  -7983,  -8155,
	-8327,  -8500,  -8672,  -8844,  -9016,  -9187,  -9359,  -9531,  -9703,  -9874,
	-10046, -10217, -10388, -10559, -10730, -10901, -11072, -11243, -11413, -11584,
	-11754, -11925, -12095, -12265, -12435, -12605, -12774, -12944, -13113, -13283,
	-13452, -13621, -13790, -13959, -14128, -14296, -14465, -14633, -14801, -14969,
	-15137, -15305, -15472, -15640, -15807, -15974, -16141, -16308, -16475, -16641,
	-16808, -16974, -17140, -17306, -17472, -17637, -17803, -17968, -18133, -18298,
	-18463, -18627, -18792, -18956, -19120, -19284, -19447, -19611, -19774, -19937,
	-20100, -20263, -20426, -20588, -20750, -20912, -21074, -21235, -21397, -21558,
	-21719, -21880, -22040, -22201, -22361, -22521, -22681, -22840, -22999, -23158,
	-23317, -23476, -23634, -23793, -23950, -24108, -24266, -24423, -24580, -24737,
	-24894, -25050, -25206, -25362, -25518, -25673, -25828, -25983, -26138, -26292,
	-26446, -26600, -26754, -26907, -27061, -27214, -27366, -27519, -27671, -27823,
	-27974, -28126, -28277, -28394, -28444, -28494, -28543, -28593, -28642, -28690,
	-28739, -28787, -28835, -28882, -28930, -28977, -29024, -29070, -29116, -29162,
	-29208, -29254, -29299, -29344, -29388, -29433, -29477, -29520, -29564, -29607,
	-29650, -29693, -29735, -29777, -29819, -29860, -29902, -29943, -29983, -30024,
	-30064, -30104, -30143, -30183, -30222, -30260, -30299, -30337, -30375, -30412,
	-30450, -30487, -30523, -30560, -30596, -30632, -30667, -30703, -30738, -30772,
	-30807, -30841, -30875, -30908, -30941, -30974, -31007, -31039, -31071, -31103,
	-31135, -31166, -31197, -31227, -31258, -31288, -31317, -31347, -31376, -31405,
	-31433, -31462, -31489, -31517, -31544, -31572, -31598, -31625, -31651, -31677,
	-31702, -31728, -31753, -31777, -31802, -31826, -31850, -31873, -31896, -31919,
	-31942, -31964, -31986, -32008, -32029, -32050, -32071, -32091, -32111, -32131,
	-32151, -32170, -32189, -32208, -32226, -32244, -32262, -32279, -32296, -32313,
	-32330, -32346, -32362, -32378, -32393, -32408, -32423, -32437, -32451, -32465,
	-32478, -32492, -32504, -32517, -32529, -32541, -32553, -32564, -32575, -32586,
	-32596, -32606, -32616, -32626, -32635, -32644, -32652, -32661, -32669, -32676,
	-32684, -32691, -32697, -32704, -32710, -32716, -32721, -32726, -32731, -32736,
	-32740, -32744, -32748, -32751, -32754, -32757, -32759, -32761, -32763, -32765,
	-32766, -32767, -32767, -32767, -32767, -32767, -32766, -32765, -32764, -32763,
	-32761, -32758, -32756, -32753, -32750, -32747, -32743, -32739, -32734, -32730,
	-32725, -32719, -32714, -32708, -32702, -32695, -32688, -32681, -32674, -32666,
	-32658, -32650, -32641, -32632, -32623, -32613, -32603, -32593, -32582, -32572,
	-32560, -32549, -32537, -32525, -32513, -32500, -32487, -32474, -32460, -32446,
	-32432, -32418, -32403, -32388, -32372, -32357, -32341, -32324, -32308, -32291,
	-32274, -32256, -32238, -32220, -32202, -32183, -32164, -32144, -32125, -32105,
	-32084, -32064, -32043, -32022, -32000, -31979, -31957, -31934, -31912, -31889,
	-31865, -31842, -31818, -31794, -31769, -31744, -31719, -31694, -31668, -31642,
	-31616, -31589, -31563, -31535, -31508, -31480, -31452, -31424, -31395, -31366,
	-31337, -31307, -31278, -31248, -31217, -31186, -31155, -31124, -31093, -31061,
	-31029, -30996, -30963, -30930, -30897, -30863, -30829, -30795, -30761, -30726,
	-30691, -30655, -30620, -30584, -30548, -30511, -30474, -30437, -30400, -30362,
	-30324, -30286, -30247, -30209, -30170, -30130, -30091, -30051, -30010, -29970,
	-29929, -29888, -29847, -29805, -29763, -29721, -29678, -29636, -29593, -29549,
	-29506, -29462, -29418, -29373, -29329, -29284, -29238, -29193, -29147, -29101,
	-29055, -29008, -28961, -28914, -28867, -28819, -28771, -28723, -28674, -28625,
	-28576, -28527, -28477, -28428, -28377, -28428, -28477, -28527, -28576, -28625,
	-28674, -28723, -28771, -28819, -28867, -28914, -28961, -29008, -29055, -29101,
	-29147, -29193, -29238, -29284, -29329, -29373, -29418, -29462, -29506, -29549,
	-29593, -29636, -29678, -29721, -29763, -29805, -29847, -29888, -29929, -29970,
	-30010, -30051, -30091, -30130, -30170, -30209, -30247, -30286, -30324, -30362,
	-30400, -30437, -30474, -30511, -30548, -30584, -30620, -30655, -30691, -30726,
	-30761, -30795, -30829, -30863, -30897, -30930, -30963, -30996, -31029, -31061,
	-31093, -31124, -31155, -31186, -31217, -31248, -31278, -31307, -31337, -31366,
	-31395, -31424, -31452, -31480, -31508, -31535, -31563, -31589, -31616, -31642,
	-31668, -31694, -31719, -31744, -31769, -31794, -31818, -31842, -31865, -31889,
	-31912, -31934, -31957, -31979, -32000, -32022, -32043, -32064, -32084, -32105,
	-32125, -32144, -32164, -32183, -32202, -32220, -32238, -32256, -32274, -32291,
	-32308, -32324, -32341, -32357, -32372, -32388, -32403, -32418, -32432, -32446,
	-32460, -32474, -32487, -32500, -32513, -32525, -32537, -32549, -32560, -32572,
	-32582, -32593, -32603, -32613, -32623, -32632, -32641, -32650, -32658, -32666,
	-32674, -32681, -32688, -32695, -32702, -32708, -32714, -32719, -32725, -32730,
	-32734, -32739, -32743, -32747, -32750, -32753, -32756, -32758, -32761, -32763,
	-32764, -32765, -32766, -32767, -32767, -32767, -32767, -32767, -32766, -32765,
	-32763, -32761, -32759, -32757, -32754, -32751, -32748, -32744, -32740, -32736,
	-32731, -32726, -32721, -32716, -32710, -32704, -32697, -32691, -32684, -32676,
	-32669, -32661, -32652, -32644, -32635, -32626, -32616, -32606, -32596, -32586,
	-32575, -32564, -32553, -32541, -32529, -32517, -32504, -32492, -32478, -32465,
	-32451, -32437, -32423, -32408, -32393, -32378, -32362, -32346, -32330, -32313,
	-32296, -32279, -32262, -32244, -32226, -32208, -32189, -32170, -32151, -32131,
	-32111, -32091, -32071, -32050, -32029, -32008, -31986, -31964, -31942, -31919,
	-31896, -31873, -31850, -31826, -31802, -31777, -31753, -31728, -31702, -31677,
	-31651, -31625, -31598, -31572, -31544, -31517, -31489, -31462, -31433, -31405,
	-31376, -31347, -31317, -31288, -31258, -31227, -31197, -31166, -31135, -31103,
	-31071, -31039, -31007, -30974, -30941, -30908, -30875, -30841, -30807, -30772,
	-30738, -30703, -30667, -30632, -30596, -30560, -30523, -30487, -30450, -30412,
	-30375, -30337, -30299, -30260, -30222, -30183, -30143, -30104, -30064, -30024,
	-29983, -29943, -29902, -29861, -29819, -29777, -29735, -29693, -29650, -29607,
	-29564, -29520, -29477, -29433, -29388, -29344, -29299, -29254, -29208, -29162,
	-29116, -29070, -29024, -28977, -28930, -28882, -28835, -28787, -28739, -28690,
	-28642, -28593, -28543, -28494, -28444, -28394, -28277, -28126, -27974, -27823,
	-27671, -27519, -27366, -27214, -27061, -26907, -26754, -26600, -26446, -26292,
	-26138, -25983, -25828, -25673, -25518, -25362, -25206, -25050, -24894, -24737,
	-24580, -24423, -24266, -24108, -23950, -23793, -23634, -23476, -23317, -23158,
	-22999, -22840, -22681, -22521, -22361, -22201, -22040, -21880, -21719, -21558,
	-21397, -21235, -21074, -20912, -20750, -20588, -20426, -20263, -20100, -19937,
	-19774, -19611, -19448, -19284, -19120, -18956, -18792, -18627, -18463, -18298,
	-18133, -17968, -17803, -17637, -17472, -17306, -17140, -16974, -16808, -16641,
	-16475, -16308, -16141, -15974, -15807, -15640, -15472, -15305, -15137, -14969,
	-14801, -14633, -14465, -14296, -14128, -13959, -13790, -13621, -13452, -13283,
	-13113, -12944, -12774, -12605, -12435, -12265, -12095, -11925, -11754, -11584,
	-11413, -11243, -11072, -10901, -10730, -10559, -10388, -10217, -10046,  -9874,
	-9703,  -9531,  -9359,  -9188,  -9016,  -8844,  -8672,  -8500,  -8327,  -8155,
	-7983,  -7810,  -7638,  -7465,  -7293,  -7120,  -6947,  -6774,  -6601,  -6428,
	-6255,  -6082,  -5909,  -5736,  -5563,  -5389,  -5216,  -5042,  -4869,  -4695,
	-4522,  -4348,  -4175,  -4001,  -3827,  -3654,  -3480,  -3306,  -3132,  -2958,
	-2784,  -2610,  -2437,  -2263,  -2089,  -1915,  -1740,  -1566,  -1392,  -1218,
	-1044,   -870,   -696,   -522,   -348,   -174,      0,    174,    348,    522,
	696,    870,   1044,   1218,   1392,   1566,   1740,   1915,   2089,   2263,
	2436,   2610,   2784,   2958,   3132,   3306,   3480,   3654,   3827,   4001,
	4175,   4348,   4522,   4695,   4869,   5042,   5216,   5389,   5563,   5736,
	5909,   6082,   6255,   6428,   6601,   6774,   6947,   7120,   7293,   7465,
	7638,   7810,   7983,   8155,   8327,   8500,   8672,   8844,   9016,   9187,
	9359,   9531,   9703,   9874,  10046,  10217,  10388,  10559,  10730,  10901,
	11072,  11243,  11413,  11584,  11754,  11925,  12095,  12265,  12435,  12605,
	12774,  12944,  13113,  13283,  13452,  13621,  13790,  13959,  14128,  14296,
	14465,  14633,  14801,  14969,  15137,  15305,  15472,  15640,  15807,  15974,
	16141,  16308,  16475,  16641,  16808,  16974,  17140,  17306,  17472,  17637,
	17803,  17968,  18133,  18298,  18463,  18627,  18792,  18956,  19120,  19284,
	19447,  19611,  19774,  19937,  20100,  20263,  20426,  20588,  20750,  20912,
	21074,  21235,  21397,  21558,  21719,  21880,  22040,  22201,  22361,  22521,
	22681,  22840,  22999,  23158,  23317,  23476,  23634,  23793,  23950,  24108,
	24266,  24423,  24580,  24737,  24894,  25050,  25206,  25362,  25518,  25673,
	25828,  25983,  26138,  26292,  26446,  26600,  26754,  26907,  27061,  27214,
	27366,  27519,  27671,  27823,  27974,  28126,  28277,  28394,  28444,  28494,
	28543,  28593,  28642,  28690,  28739,  28787,  28835,  28882,  28930,  28977,
	29024,  29070,  29116,  29162,  29208,  29254,  29299,  29344,  29388,  29433,
	29477,  29520,  29564,  29607,  29650,  29693,  29735,  29777,  29819,  29860,
	29902,  29943,  29983,  30024,  30064,  30104,  30143,  30183,  30222,  30260,
	30299,  30337,  30375,  30412,  30450,  30487,  30523,  30560,  30596,  30632,
	30667,  30703,  30738,  30772,  30807,  30841,  30875,  30908,  30941,  30974,
	31007,  31039,  31071,  31103,  31135,  31166,  31197,  31227,  31258,  31288,
	31317,  31347,  31376,  31405,  31433,  31462,  31489,  31517,  31544,  31572,
	31598,  31625,  31651,  31677,  31702,  31728,  31753,  31777,  31802,  31826,
	31850,  31873,  31896,  31919,  31942,  31964,  31986,  32008,  32029,  32050,
	32071,  32091,  32111,  32131,  32151,  32170,  32189,  32208,  32226,  32244,
	32262,  32279,  32296,  32313,  32330,  32346,  32362,  32378,  32393,  32408,
	32423,  32437,  32451,  32465,  32478,  32492,  32504,  32517,  32529,  32541,
	32553,  32564,  32575,  32586,  32596,  32606,  32616,  32626,  32635,  32644,
	32652,  32661,  32669,  32676,  32684,  32691,  32697,  32704,  32710,  32716,
	32721,  32726,  32731,  32736,  32740,  32744,  32748,  32751,  32754,  32757,
	32759,  32761,  32763,  32765,  32766,  32767,  32767,  32767,  32767,  32767,
	32766,  32765,  32764,  32763,  32761,  32758,  32756,  32753,  32750,  32747,
	32743,  32739,  32734,  32730,  32725,  32719,  32714,  32708,  32702,  32695,
	32688,  32681,  32674,  32666,  32658,  32650,  32641,  32632,  32623,  32613,
	32603,  32593,  32582,  32572,  32560,  32549,  32537,  32525,  32513,  32500,
	32487,  32474,  32460,  32446,  32432,  32418,  32403,  32388,  32372,  32357,
	32341,  32324,  32308,  32291,  32274,  32256,  32238,  32220,  32202,  32183,
	32164,  32144,  32125,  32105,  32084,  32064,  32043,  32022,  32000,  31979,
	31957,  31934,  31912,  31889,  31865,  31842,  31818,  31794,  31769,  31744,
	31719,  31694,  31668,  31642,  31616,  31589,  31563,  31535,  31508,  31480,
	31452,  31424,  31395,  31366,  31337,  31307,  31278,  31248,  31217,  31186,
	31155,  31124,  31093,  31061,  31029,  30996,  30963,  30930,  30897,  30863,
	30829,  30795,  30761,  30726,  30691,  30655,  30620,  30584,  30548,  30511,
	30474,  30437,  30400,  30362,  30324,  30286,  30247,  30209,  30170,  30130,
	30091,  30051,  30010,  29970,  29929,  29888,  29847,  29805,  29763,  29721,
	29678,  29636,  29593,  29549,  29506,  29462,  29418,  29373,  29329,  29284,
	29238,  29193,  29147,  29101,  29055,  29008,  28961,  28914,  28867,  28819,
	28771,  28723,  28674,  28625,  28576,  28527,  28477,  28428,  28377, 
};

/* lsb: 1/2048 */
const sint16 sine_table[] = {
	    	0,    13,    25,    38,    50,    63,    75,    88,   100,   113,
		  126,   138,   151,   163,   176,   188,   201,   213,   226,   238,
		  251,   263,   276,   288,   301,   313,   325,   338,   350,   363,
		  375,   387,   400,   412,   424,   436,   449,   461,   473,   485,
		  498,   510,   522,   534,   546,   558,   570,   582,   595,   607,
		  619,   630,   642,   654,   666,   678,   690,   702,   714,   725,
		  737,   749,   760,   772,   784,   795,   807,   818,   830,   841,
		  853,   864,   876,   887,   898,   910,   921,   932,   943,   954,
		  965,   976,   988,   999,  1009,  1020,  1031,  1042,  1053,  1064,
		 1074,  1085,  1096,  1106,  1117,  1127,  1138,  1148,  1159,  1169,
		 1179,  1190,  1200,  1210,  1220,  1230,  1240,  1250,  1260,  1270,
		 1280,  1289,  1299,  1309,  1319,  1328,  1338,  1347,  1357,  1366,
		 1375,  1385,  1394,  1403,  1412,  1421,  1430,  1439,  1448,  1457,
		 1466,  1475,  1483,  1492,  1500,  1509,  1517,  1526,  1534,  1543,
		 1551,  1559,  1567,  1575,  1583,  1591,  1599,  1607,  1615,  1622,
		 1630,  1637,  1645,  1652,  1660,  1667,  1674,  1682,  1689,  1696,
		 1703,  1710,  1717,  1724,  1730,  1737,  1744,  1750,  1757,  1763,
		 1769,  1776,  1782,  1788,  1794,  1800,  1806,  1812,  1818,  1824,
		 1829,  1835,  1840,  1846,  1851,  1857,  1862,  1867,  1872,  1877,
		 1882,  1887,  1892,  1897,  1902,  1906,  1911,  1915,  1920,  1924,
		 1928,  1932,  1937,  1941,  1945,  1949,  1952,  1956,  1960,  1963,
		 1967,  1970,  1974,  1977,  1980,  1984,  1987,  1990,  1993,  1995,
		 1998,  2001,  2004,  2006,  2009,  2011,  2013,  2016,  2018,  2020,
		 2022,  2024,  2026,  2028,  2029,  2031,  2033,  2034,  2036,  2037,
		 2038,  2039,  2040,  2041,  2042,  2043,  2044,  2045,  2046,  2046,
		 2047,  2047,  2047,  2048,  2048,  2048,  2048,
};

/* table physical value: 0 ~ pi/4, internal value: 0 ~ 8092; LSB: 2pi/65536 */
const uint16 atan_table[] =
{
	    	0,    81,   163,   244,   326,   407,   489,   570,   651,   732,
		  813,   894,   975,  1056,  1136,  1217,  1297,  1377,  1457,  1537,
		 1617,  1696,  1775,  1854,  1933,  2012,  2090,  2168,  2246,  2324,
		 2401,  2478,  2555,  2632,  2708,  2784,  2860,  2935,  3010,  3085,
		 3159,  3233,  3307,  3380,  3453,  3526,  3598,  3670,  3742,  3813,
		 3884,  3955,  4025,  4095,  4164,  4233,  4302,  4370,  4438,  4505,
		 4572,  4639,  4705,  4771,  4836,  4901,  4966,  5030,  5093,  5157,
		 5220,  5282,  5344,  5406,  5467,  5528,  5589,  5649,  5708,  5768,
		 5826,  5885,  5943,  6000,  6057,  6114,  6171,  6226,  6282,  6337,
		 6392,  6446,  6500,  6554,  6607,  6660,  6712,  6764,  6815,  6867,
		 6917,  6968,  7018,  7067,  7117,  7166,  7214,  7262,  7310,  7357,
		 7405,  7451,  7498,  7544,  7589,  7634,  7679,  7724,  7768,  7812,
		 7856,  7899,  7942,  7984,  8026,  8068,  8110,  8151,  8192,  8192
};


void comath_xy_to_polar_coordinates(sint16 x, sint16 y, sint16 *rou, uint16 *theta) {
	sint16 sine_value;
	sint16 cos_value;
	uint16 x_abs;
	uint16 y_abs;
	
	x_abs = calc_abs(x);
	y_abs = calc_abs(y);
	
	if (x_abs == 0 && y_abs == 0) {
		*rou = 0;
		*theta = 0;
	}
	else {
		*theta = comath_atan_lookup(y, x);
		comath_sin_cos_lookup_table(((*theta) >> 6), &sine_value, &cos_value);
		if (x_abs > y_abs) {
			*rou = ((sint32)x_abs << 11) / cos_value;
		}
		else {
			*rou = ((sint32)y_abs << 11) / sine_value;
		}
		*rou = calc_abs(*rou);
	}
}

sint16 svm_lookup_table_hd(uint16 theta) {
	uint16 index = theta >> SVM_SHIFT_BITS;
	sint16 y1 = comath_svm_table[index];
	sint16 y2 = comath_svm_table[index + 1];
	uint16 delta_x = theta & ((1 << SVM_SHIFT_BITS) - 1);
	sint16 result;

	result = y1 + (((sint32)(y2 - y1) * (sint32)delta_x) >> SVM_SHIFT_BITS);
	return result;
}

/* k: lsb: 1/32768 */
void first_order_filter_init(FisrtOrderFilt_T *first_order_flt, int16 input_init, int16 k) {
	first_order_flt->out_hd = (int32)input_init << 15;
	first_order_flt->coef = (int32)k;
}

int16 first_order_filter(FisrtOrderFilt_T *first_order_flt, int16 input) {
	first_order_flt->out_hd = first_order_flt->coef * (int32)input + (32768 - first_order_flt->coef) * (first_order_flt->out_hd >> 15);
	return (first_order_flt->out_hd >> 15);
}

int32 speed_ramp(int32 dest_speed, int32 curr_speed, int32 accelerate) {
	int32 output = curr_speed;
	if (curr_speed < dest_speed) {
		output += accelerate;
		if (output > dest_speed) {
			output = dest_speed;
		}
	}
	else if (curr_speed > dest_speed) {
		output -= accelerate;
		if (output < dest_speed) {
			output = dest_speed;
		}
	}
	else {
	}
	return output;
}

int16 linear_interprotion_xint16_yint16(const int16 *x_axis, const int16 *y_axis, uint16 axis_len, int16 x) {
	int16 result;
	uint16 i;
	uint16 index;
	if (x <= x_axis[0]) {
		result = y_axis[0];
	}
	else if (x >= x_axis[axis_len - 1]) {
		result = y_axis[axis_len - 1];
	}
	else {
		for (i = 0; i < axis_len; i++) {
			if (x < x_axis[i]) {
				index = i - 1;
				break;
			}
		}
		result = y_axis[index] + ((int32)y_axis[index + 1] - y_axis[index]) * ((int32)x - x_axis[index]) / (x_axis[index + 1] - x_axis[index]);
	}
	return result;
}

int16 linear_interprotion_xuint16_yint16(const uint16 *x_axis, const int16 *y_axis, uint16 axis_len, uint16 x) {
	int16 result;
	uint16 i;
	uint16 index;
	if (x <= x_axis[0]) {
		result = y_axis[0];
	}
	else if (x >= x_axis[axis_len - 1]) {
		result = y_axis[axis_len - 1];
	}
	else {
		for (i = 0; i < axis_len; i++) {
			if (x < x_axis[i]) {
				index = i - 1;
				break;
			}
		}
		result = y_axis[index] + ((int32)y_axis[index + 1] - y_axis[index]) * ((int32)x - (int32)x_axis[index]) / ((int32)x_axis[index + 1] - (int32)x_axis[index]);
	}
	return result;
}

uint16 linear_interprotion_xuint16_yuint16(const uint16 *x_axis, const uint16 *y_axis, uint16 axis_len, uint16 x) {
	uint16 result;
	uint16 i;
	uint16 index;
	if (x <= x_axis[0]) {
		result = y_axis[0];
	}
	else if (x >= x_axis[axis_len - 1]) {
		result = y_axis[axis_len - 1];
	}
	else {
		for (i = 0; i < axis_len; i++) {
			if (x < x_axis[i]) {
				index = i - 1;
				break;
			}
		}
		result = y_axis[index] + ((sint32)y_axis[index + 1] - y_axis[index]) * ((sint32)x - x_axis[index]) / (x_axis[index + 1] - x_axis[index]);
	}
	return result;
}


void pi_controller_init(PiCtrlData_T* pi_controller, uint16 Kp, uint16 Ki, int16 sum_min, int16 sum_max, int16 out_min, int16 out_max, uint16 scaling_factor) {
    /* initialise state */
    pi_controller->state.sum_error = 0;
    pi_controller->state.value_of_p = 0;
    //pi_controller->state.norm = multiplier;

    /* initialise parameter */
    pi_controller->para.Kp = Kp;
    pi_controller->para.Ki = Ki;
    pi_controller->para.scaling_factor = scaling_factor;
    pi_controller->para.sum_max = ((int32)sum_max) << scaling_factor;
    pi_controller->para.sum_min = ((int32)sum_min) << scaling_factor;
    pi_controller->para.out_max = ((int32)out_max) << scaling_factor;
    pi_controller->para.out_min = ((int32)out_min) << scaling_factor;
}

void pi_controller_reset( PiCtrlData_T* pi_controller, int16 current_value) {
    pi_controller->state.sum_error 	= ((sint32)current_value << pi_controller->para.scaling_factor);
    pi_controller->state.value_of_p = 0;
}

int32 pi_controller_calc(PiCtrlData_T* pi, int16 error) {
    int32 y_p; 
    int32 y_i;
    int32 diff;
    int32 result;
    
    /*************** I-term ***************/
    diff =  (int32)pi->para.Ki * (int32)error;
    y_i = pi->state.sum_error + diff;
    /*************** Limitation ***************/
	if (y_i > pi->para.sum_max) {
		y_i = pi->para.sum_max;
	}
	if (y_i < pi->para.sum_min ) {
		y_i = pi->para.sum_min;
	}
    /*************** P-term ***************/
    y_p = ((int32)pi->para.Kp * (int32)error);

	/* y_i and y_p copy for further limitations */
    pi->state.sum_error = y_i;
    pi->state.value_of_p = y_p;

    /* Calculate return value */
    result = y_p + y_i;
    if (result > pi->para.out_max) {
    	result = pi->para.out_max;
    }
    else if (result < pi->para.out_min) {
    	result = pi->para.out_min;
    }
    else {
    	/* no operation */
    }
    result = (result >> pi->para.scaling_factor);
    return result;
}

void pi_anti_windup_controller_init(PiAwCtrlData_T* piAw_controller, uint16 Kp, uint16 Ki, uint16 Kb, int16 i_min, int16 i_max, int16 out_min, int16 out_max, uint16 scaling_factor) {
    /* initialise state */
	piAw_controller->state.sum_error = 0;
	piAw_controller->state.value_of_p = 0;
	piAw_controller->state.sat_diff = 0;
	piAw_controller->state.result = 0;
    //pi_controller->state.norm = multiplier;

    /* initialise parameter */
	piAw_controller->para.Kp = Kp;
	piAw_controller->para.Ki = Ki;
	piAw_controller->para.Kb = Kb;
	piAw_controller->para.scaling_factor = scaling_factor;
	piAw_controller->para.i_max = ((int32)i_max) << scaling_factor;
	piAw_controller->para.i_min = ((int32)i_min) << scaling_factor;
	piAw_controller->para.out_max = ((int32)out_max) << scaling_factor;
	piAw_controller->para.out_min = ((int32)out_min) << scaling_factor;
}

void pi_anti_windup_controller_reset( PiAwCtrlData_T* piAw_controller, int16 current_value) {
	piAw_controller->state.sum_error 	= ((sint32)current_value << piAw_controller->para.scaling_factor);
    piAw_controller->state.value_of_p = 0;
    piAw_controller->state.sat_diff = 0;
	piAw_controller->state.result = 0;
}

/* TODO: NOT verified */
int32 pi_anti_windup_controller_calc(PiAwCtrlData_T* piAw, int16 error) {
    int32 y_i;
    int32 y_before_sat;
    int32 y_out;
    int32 result;
    
    /*************** I-term ***************/
    y_i =  (int32)piAw->para.Ki * (int32)error + (int32)piAw->para.Kb * piAw->state.sat_diff;
    piAw->state.sum_error += y_i;
	if (piAw->state.sum_error > piAw->para.i_max) {
		piAw->state.sum_error = piAw->para.i_max;
	}
	else if (piAw->state.sum_error < piAw->para.i_min ) {
		piAw->state.sum_error = piAw->para.i_min;
	}
	else {
		/* no operation */
	}
    /*************** P-term ***************/
    piAw->state.value_of_p = ((int32)piAw->para.Kp * (int32)error);
    /************** value out *************/
    y_before_sat = piAw->state.sum_error + piAw->state.value_of_p;
    if (y_before_sat > piAw->para.out_max) {
    	y_out = piAw->para.out_max;
    }
    else if (y_before_sat < piAw->para.out_min) {
    	y_out = piAw->para.out_min;
    }
    else {
    	y_out = y_before_sat;
    }
    piAw->state.sat_diff = y_out - y_before_sat;
    piAw->state.result = y_out;

    result = (y_out >> piAw->para.scaling_factor);
    return result;
}

void pid_controller_init(PidCtrlData_T* pid_controller, uint16 Kp, uint16 Ki, uint16 Kd, int16 sum_min, int16 sum_max, int16 out_min, int16 out_max, uint16 scaling_factor) {
    /* initialise state */
	pid_controller->state.sum_error = 0;
	pid_controller->state.value_of_p = 0;
	pid_controller->state.d_pre = 0;
    //pi_controller->state.norm = multiplier;

    /* initialise parameter */
	pid_controller->para.Kp = Kp;
	pid_controller->para.Ki = Ki;
	pid_controller->para.Kd = Kd;
	pid_controller->para.scaling_factor = scaling_factor;
	pid_controller->para.sum_max = ((int32)sum_max) << scaling_factor;
	pid_controller->para.sum_min = ((int32)sum_min) << scaling_factor;
	pid_controller->para.out_max = ((int32)out_max) << scaling_factor;
	pid_controller->para.out_min = ((int32)out_min) << scaling_factor;
}

void pid_controller_reset( PidCtrlData_T* pid_controller, int16 current_value) {
	pid_controller->state.sum_error 	= ((sint32)current_value << pid_controller->para.scaling_factor);
	pid_controller->state.value_of_p = 0;
}

int32 pid_controller_calc(PidCtrlData_T* pid_controller, int16 error) {
    int32 y_p; 
    int32 y_i;
    int32 y_d;
    int32 diff;
    int32 result;
    
    /*************** I-term ***************/
    diff =  (int32)pid_controller->para.Ki * (int32)error;
    y_i = pid_controller->state.sum_error + diff;
    /*************** Limitation ***************/
	if (y_i > pid_controller->para.sum_max) {
		y_i = pid_controller->para.sum_max;
	}
	if (y_i < pid_controller->para.sum_min ) {
		y_i = pid_controller->para.sum_min;
	}
    /*************** P-term ***************/
    y_p = ((int32)pid_controller->para.Kp * (int32)error);
    /*************** D-term ***************/
    y_d = (int32)pid_controller->para.Kd * (error - pid_controller->state.d_pre);
    pid_controller->state.d_pre = error;
	/* y_i and y_p copy for further limitations */
    pid_controller->state.sum_error = y_i;
    pid_controller->state.value_of_p = y_p;

    /* Calculate return value */
    result = y_p + y_i + y_d;
    if (result > pid_controller->para.out_max) {
    	result = pid_controller->para.out_max;
    }
    else if (result < pid_controller->para.out_min) {
    	result = pid_controller->para.out_min;
    }
    else {
    	/* no operation */
    }
    result = (result >> pid_controller->para.scaling_factor);
    return result;
}

void comath_output_limit_delay_init(OutputLimitDelay_T *limit_delay, 
		EnterType enter_type,
		sint16 thres_hi, 
		sint16 thres_lo, 
		uint16 enter_counter_max, 
		uint16 escape_counter_max) {
	limit_delay->control.result = false;
	limit_delay->control.enter_count = 0;
	limit_delay->control.escape_count = 0;
	limit_delay->para.enter_type = enter_type;
	limit_delay->para.thres_hi = thres_hi;
	limit_delay->para.thres_lo = thres_lo;
	limit_delay->para.enter_counter_max = enter_counter_max;
	limit_delay->para.escape_counter_max = escape_counter_max;
}

bool comath_ouput_limitation_delay_calc(OutputLimitDelay_T *limit_delay, sint16 input) {
	if (limit_delay->para.enter_type == ENTER_FROM_LO_TO_HI) {
		if (input > limit_delay->para.thres_hi) {
			if (limit_delay->control.enter_count < limit_delay->para.enter_counter_max) {
				limit_delay->control.enter_count++;
			}			
		}
		else {
			limit_delay->control.enter_count = 0;
		}
		if (input < limit_delay->para.thres_lo) {
			if (limit_delay->control.escape_count < limit_delay->para.escape_counter_max) {
				limit_delay->control.escape_count++;
			}
		}
		else {
			limit_delay->control.escape_count = 0;
		}
	}
	else {
		if (input < limit_delay->para.thres_lo) {
			if (limit_delay->control.enter_count < limit_delay->para.enter_counter_max) {
				limit_delay->control.enter_count++;
			}			
		}
		else {
			limit_delay->control.enter_count = 0;
		}
		if (input > limit_delay->para.thres_hi) {
			if (limit_delay->control.escape_count < limit_delay->para.escape_counter_max) {
				limit_delay->control.escape_count++;
			}
		}
		else {
			limit_delay->control.escape_count = 0;
		}
	}
	if (limit_delay->control.result == true) {
		if (limit_delay->control.escape_count >= limit_delay->para.escape_counter_max) {
			limit_delay->control.result = false;
		}
	}
	else {
		if (limit_delay->control.enter_count >= limit_delay->para.enter_counter_max) {
			limit_delay->control.result = true;
		}
	}
	return limit_delay->control.result;
}

/* theta lsb: 2p/1024 */
void comath_sin_cos_lookup_table(sint16 theta, sint16 *sine_value, sint16 *cos_value) {
	sint16 index = theta >> 8;
	sint16 remain = theta & 0xFF;

	switch (index) {
	case 0:
		*sine_value = sine_table[remain];
		*cos_value = sine_table[256 - remain];
		break;
	case 1:
		*sine_value = sine_table[256 - remain];
		*cos_value = -sine_table[remain];
		break;
	case 2:
		*sine_value = -sine_table[remain];
		*cos_value = -sine_table[256 - remain];
		break;
	case 3:
		*sine_value = -sine_table[256 - remain];
		*cos_value = sine_table[remain];
		break;
	default :
		*sine_value = 0;
		*cos_value = 0;
		break;
	}
}

sint16 comath_atan_lookup(sint16 y, sint16 x) {
	uint16 phi;
	uint16 t, fa, fb, h;

	if (x == 0) {
		phi = (y >= 0 ? COMATH_DEGREE_90 : COMATH_DEGREE_270);
	}
	else if (y == 0) {
		phi = (x >= 0 ? 0 : COMATH_DEGREE_180);
	}
	else {
		OCTANTIFY(x, y, phi);
		phi = ((uint32)phi * COMATH_DEGREE_180 + 2) >> 2;

		t = ((sint32)y << 14) / x;

		h = t & 0x7F;
		fa = atan_table[t >> 7];
		fb = atan_table[(t >> 7) + 1];
		phi += (fa + ((fb - fa) * h >> 7));
	}

	return phi;
}
